COMMENT ⊗   VALID 00005 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	put locations into comtab
C00003 00003	 routines to set up coefficients for drive
C00010 00004	ROUTINE TO READ RAW FORCE WRIST AND RETURN INFORMATION TO PDP10
C00012 00005		setspeed
C00013 ENDMK
C⊗;
;put locations into comtab

DATA
PUTLOC	LSETSPEED,SETSPEED
PUTLOC	LRFORCE, RFORCE
PUTLOC	LDRV0,DRV0
PUTLOC	LDRIVE,DRIVE
CODE
; routines to set up coefficients for drive
;	DRV0 is a routine that computes the appropriate coefficients
;	given the arm and joint number and the amount of motion and
;	whether it is absolute or relative.
;	AC0	=  some amount
;	r3	=  joint number
;	r1	=  arm number
;	r2	=  absolute (1) or relative (0)
	⊗

DRV0:	MOV	R4,-(SP)
	MOV	R5,-(SP)
	MOV	R0,CFLST	; save coeff list address
	MOV	R3,JOINT
	MOV	R0,R3		; r3 now will build up polylist
	MOV	R2,ABSDRV	; save r2
	STF	AC0,JTAMT	; save r0
	TST	R1		; check which arm
	BEQ	1$		; skip if yellow
	MOV	#400,R0		;THIS IS BLUE ARM JOINT 1 SERVO BIT
	BR	2$
1$:	MOV	#100000,R0	;THIS IS YELLOW ARM JOINT 1 SERVO BIT
2$:	MOV	JOINT,R1	;GET THE JOINT NUMBER 1-7
	DEC	R1     
       	NEG	R1
	ASH	R1,R0		;SHIFT BIT TO INDICATE PROPER JOINT 
       	MOV	R0,(R3)+	;SET SERVO BIT
	CLR	(R3)+		;zero next servo word
	CLR	(R3)+		;no special word
	CLR	(R3)+		;no wobble
	MOV	#40.,(R3)+	;rel segment pointer
	MOV	#2000.,(R3)+	;SET DURATION OF DRIVE MOTION = 2000 MSEC
	CLR	(R3)+		; no trans
	CLR	(R3)+		;no run code
	MOV	R3,-(SP)	; save the address so far

;SET UP THE DYNAMIC COEFFICIENT LIST CI AND CII

	BIT	#YARM+YHAND,@CFLST	;CHECK IF THIS IS FOR THE YELLOW ARM
	BEQ	NOYEL
	MOV	#YARM+YHAND,WLST	;IF SO, READ THE CURRENT JOINT ANGLES
	MOV	#YELARM,R2	;INDICATE YELLOW ARM
	MOV	#YTH,R4		;GET POINTER TO YELLOW JOINT ANGLES
	BR	FILLAN
;*k could be vise too
NOYEL:	MOV	#BARM+BHAND,WLST	;ELSE MUST BE BLUE ARM
	MOV	#BLUARM,R2	;INDICATE BLUE ARM
	MOV	#BTH,R4		;GET POINTER TO BLUE JOINT ANGLES
FILLAN:	MOV	#WLST,R0
	MOV	#DEVICE,R1
	JSR	PC,WHERE
	BIT	#YHAND+BHAND,@CFLST	;CHECK IF HAND SELECTED FOR DRIVE
	BNE	ISHND		;BRANCH IF HAND OPERATION
	MOV	JOINT,R3	;GET POINTER TO JOINT ANGLE
	DEC	R3
	ASH	#1,R3
	ADD	R4,R3
	
	LDF	JTAMT,AC0
	LDF	@(R3),AC1	;AC1←OLD VALUE
	STF	AC1,JTOLD	;save old value of joint
	TST	ABSDRV
	BEQ	2$		; branch because relative
	SUBF	AC0,AC1		;AC1←DIFFERENCE
	NEGF	AC1
	STF	AC1,JTDIF	
	BR	3$
2$:	STF	AC0,JTDIF	
3$:	MOV	R4,R0		;COMPUTE CORRESPONDING ARM DYNAMIC COEFFICIENTS
	MOV	#CIPTR,R1
	JSR	PC,DTERMS
	MOV	JOINT,R3	;GET INDEX TO DYNAMIC COEFFICIENTS
	ASH	#2,R3
	LDF	CILST-4(R3),AC0	;PICK UP GRAVITY LOADING AND INERTIA
	STF	AC0,SEGCI
	LDF	CIILST-4(R3),AC0
	STF	AC0,SEGCII
      	MOV	#WLST,R0	;RESTORE CURRENT JOINT ANGLES
	MOV	#DEVICE,R1
	JSR	PC,WHERE
	BR	DODRVE
ISHND:
	BIT	#YHAND,@CFLST	;CHECK IF YELLOW HAND
	BEQ	NOYHND
	LDF	CI+SRV07,AC0	;IF YELLOW GET PROPER CI AND CII
	LDF	CII+SRV07,AC1
	BR	DOHAND
NOYHND:	BIT	#BHAND,@CFLST	;CHECK IF BLUE HAND
	BEQ	NOBHND
	LDF	CI+SRV14,AC0	;IF BLUE GET PROPER CI AND CII
	LDF	CII+SRV14,AC1
	BR	DOHAND
NOBHND:	BIT	#VISE,@CFLST	;CHECK IF VISE
	BEQ	NOVISE
;	LDF	CI+SRV15,AC0	;IF VISE GET PROPER CI AND CII
;	LDF	CII+SRV15,AC1
	BR 	DOHAND
;;; should not come here
NOVISE:
;;	MOV	#NSCR,R0	;SAY LOOKING FOR SCREWDRIVER
;;	JSR	PC,TYPSTR
;;	DISMIS
DOHAND:	STF	AC0,SEGCI	;PUT DYNAMIC COEF. IN DATA LIST
	STF	AC1,SEGCII

;fill up rest of coefficients

DODRVE:	MOV	(SP)+,R3	;restore state of coef list
	MOV	#6,R1
4$:	CLR	(R3)+		;next tHREE coeffs=0
	SOB	R1,4$
	LDF	JTDIF,AC0	;GET THE CHANGE IN JOINT ANGLE
	LDF	C10,AC1		;SCALE 5TH ORDER POLYNOMIAL BY CHANGE AND PACK
	MULF	AC0,AC1		;  IN TO COEFFICIENT DATA LIST
	STF	AC1,(R3)+
	LDF	CM15,AC1
	MULF	AC0,AC1
	STF	AC1,(R3)+
	LDF	C6,AC1
	MULF	AC0,AC1
	STF	AC1,(R3)+
	LDF	SEGCI,AC0
	STF	AC0,(R3)+
	LDF	SEGCII,AC0
	STF	AC0,(R3)+
	CLR	(R3)+
	CLR	(R3)+
	MOV	(SP)+,R5
	MOV	(SP)+,R4
	RTS	PC

DATA
ABSDRV:	.WORD	0
JOINT:	.WORD	0
JTDIF:	.WORD	0
DEVICE:	.BLKW	32	;DEVICE BLOCK
CFLST:	.WORD	0		;coefficient list
SEGCI:	.BLKW	2		;CI
SEGCII:	.BLKW	2		;CII
WLST:	374     ;JOINT SERVO BIT
	0
C10:	.WORD	41040,0		;10.0
CM15:	.WORD	141160,0	;-15.0
C6:	.WORD	40700,0		;6.0

;TABLES FOR COMPUTE DYNAMIC COEFFICIENTS

CIPTR:	CILST
	CILST+4
	CILST+10
	CILST+14
	CILST+20
	CILST+24
	CIILST
	CIILST+4
	CIILST+10
	CIILST+14
	CIILST+20
	CIILST+24
CILST:	.BLKW	12.
CIILST:	.BLKW	12.
JTOLD:	.WORD 0
JTAMT:	.WORD	0,0
NSCR:	.ASCIZ	/LOOKING FOR THE SCREWDRIVER
/
CODE
;ROUTINE TO READ RAW FORCE WRIST AND RETURN INFORMATION TO PDP10
; copied over from old POINTY file
; note that the data sent is in integer format
;
; procedure should be called with R1 containing a pointer to
; the buffer in which the data is to be stored.
; sample call is as follows:
;
;	MOV	#FPPTR,R1
;	JSR	PC,RFORCE
;
;	R0,R2 will be garbaged.
;	FPPTR will be updated
;
RFORCE:	MOV	R3,-(SP)	;save registers
	MOV	R4,-(SP)
	MOV	R1,-(SP)
	MOV	(R1),R1		;now R1 has address instead of address of address

	MOV	#10.,R2		;READ 10 SETS OF DATA

SETLP:	MOV	#9.,R3		;EIGHT STRAIN GAGES IN ALL+REF CHAN
	MOV	#30,R4		;FIRST STRAIN GAGE CHANNEL
REDLP:	MOVB	R4,ADCCHN	;START CONVERTING STRAIN GAGE READING
WLP:    TSTB	ADCSR		;WAIT TILL CONVERSION COMPLETED
	BMI	CNVDNE  
	BR 	WLP
CNVDNE:	MOV	ADCVAL,R0	;GET READING FROM BLUE INTERFACE
;	ADD	#2048.,R0
	MOV	R0,(R1)+	;SAVE READING
	INC	R4		;POINT TO NEXT CHANNEL
	SOB	R3,REDLP	;REPEAT UNTIL DONE
	CLR	-2(R1)		;NO REFERENCE READING
	SOB	R2,SETLP	;DO IT 10. TIMES

	MOV	(SP)+,R0	;now update the pointer value
	MOV	R1,(R0)
	MOV	(SP)+,R4	;retrieve registers
	MOV	(SP)+,R3
	RTS	PC
;	setspeed

SETSPEED:
	STF AC0,SPEED	; change the speed factor
	RTS PC
DATA
SPEED:	 .FLT2	2.0			;DEFAULT SPEED FACTOR
CODE